package naito_rescue.router;

import rescuecore2.misc.*;
import rescuecore2.misc.geometry.*;
import rescuecore2.worldmodel.*;
import rescuecore2.standard.entities.*;

import naito_rescue.*;
import naito_rescue.agent.*;
import naito_rescue.object.*;
import java.util.*;

import static naito_rescue.debug.DebugUtil.*;

public final class NAITORouter{
	private StandardWorldModel model;
	private NAITOAgent         owner;
	private MyLogger           logger;
	
	//ArrayList<Area> OPEN, CLOSED;
	//HashMap<Area, Integer> estimates;
	//HashMap<Area, Area>    ancestors;

	PassableChecker checker;
	
    public NAITORouter(NAITOAgent owner) {
		this.owner = owner;
		this.model = owner.getWorldModel();
		this.logger = this.owner.getLogger();
		
		//OPEN = new ArrayList<Area>();
		//CLOSED = new ArrayList<Area>();
		//estimates = new HashMap<Area, Integer>();
		//ancestors = new HashMap<Area, Area>();
		
		checker = new PassableChecker(owner);
	}

    public List<EntityID> breadthFirstSearch(StandardEntity start, StandardEntity... goals) {
        return breadthFirstSearch(start, Arrays.asList(goals));
    }
    public List<EntityID> getRoute(StandardEntity goal){
    /*
    	//logger.info("");
    	//logger.info("+*+*+*+*+*+*+*+* NAITORouter.getRoute(); +*+*+*+*+*+*+*+*");
    	//logger.info("breadthFirstSearch(" + owner.getLocation() + ", " + goal);
    	List<EntityID> path1 = breadthFirstSearch(owner.getLocation(), goal);
    	//logger.info("breadthFirstSearch Result: " + path1);
    	//logger.info("AStar(" + owner.getLocation() + ", " + goal);
    	List<EntityID> path2 = AStar(owner.getLocation(), goal);
    	//logger.info("AStar Result: " + path2);
    */
    	return AStar(owner.getLocation(), goal);
    }

	public List<EntityID> AStar(StandardEntity start, StandardEntity goal){
		String context = start + ", " + goal + "";
		//logger.info("///// NAITORouter.AStar(" + context + ") /////");
		
		List<NAITOArea> OPEN   = new ArrayList<NAITOArea>();
		List<NAITOArea> CLOSED = new ArrayList<NAITOArea>();
		Map<Area, Area> ancestors = new HashMap<Area, Area>();
		
		Area startArea = null, goalArea = null;
		if(start instanceof Area && goal instanceof Area){
			startArea = (Area)(start);
			goalArea = (Area)(goal);
			double cost = estimateCost(ancestors, startArea, startArea, goalArea);
			
			NAITOArea area = (NAITOArea)(owner.allNAITOAreas.get(start.getID()));
			area.setEstimateCost(cost);
			OPEN.add(area);
		}else{
			//logger.info("start (" + start + ") or goal (" + goal + ") is null. return false;");
			//logger.info("///// NAITORouter.AStar(" + context + ") end. /////");
			return null;
		}
		
		boolean found = false;
		ancestors.put(startArea, startArea);
		
		int whilecount = 0;
		pathfind:
		do{
			++whilecount;
			//logger.info("");
			//logger.info(whilecount + " 's Loop Start.");
			//logger.info("OPEN Before Remove: " + OPEN);
			NAITOArea nCurrent = removeMinimumCostArea(OPEN);//OPENから，コストの見積りが最小のエリアを取り出す)
			if(!CLOSED.contains(nCurrent)){
				//logger.info("CLOSED.add(" + nCurrent.getID().getValue() + ").");
				CLOSED.add(nCurrent);
			}
			//logger.info("OPEN After Remove: " + OPEN);
			//logger.info("Minimum Cost Estimated Area = " + nCurrent.getStandardArea() + ", cost = " + nCurrent.getEstimateCost());
			if(nCurrent == null){
				//logger.info("break; oh...");
				break pathfind; //そんなもんはないのでループ終了
			}
			if(nCurrent.getID().getValue() == goal.getID().getValue()){
				// Goal.
				//logger.info("Path is FOUND.");
				found = true;
				break pathfind;
			}
			List<Area> areas = getNeighbours(nCurrent.getStandardArea());
			//logger.info("Checking Neighbour: [" + nCurrent.getStandardArea().getID().getValue() + "] => [ " + areas + " ]");
			for(Area area : areas){
				//logger.info("Now Checking Neighbour = [" + area.getID().getValue() + "] (Current=[" + nCurrent.getStandardArea().getID().getValue() + "])");
				if(area.getID().getValue() == goal.getID().getValue()){
					//logger.info("Path is FOUND(in Checking Neighbour)");
					ancestors.put(area, nCurrent.getStandardArea());
					found = true;
					break pathfind;
				}else if(area instanceof Building){
					continue;
				}
				NAITOArea nArea = (NAITOArea)(owner.allNAITOAreas.get(area.getID())); //area == nArea
				if(!ancestors.containsKey(nArea.getStandardArea()))
					ancestors.put(nArea.getStandardArea(), nCurrent.getStandardArea());
				double newCost = estimateCost(ancestors, startArea, area, goalArea); //コストの見積り計算
				//logger.info("*Current=[" + nCurrent.getID().getValue() + "]->To[" + area.getID().getValue() + "],cost=" + newCost);
				if(newCost == -1){
					//logger.info("Unreachable!!(in AStar())");
					continue;
				}
				if(!OPEN.contains(nArea) && !CLOSED.contains(nArea)){
					//logger.info("OPEN.add(" + nArea + ");");
					OPEN.add(nArea);
					nArea.setEstimateCost(newCost);
					//logger.info("Set Ancestor: " + nArea + " => " + nCurrent);
					if(!ancestors.containsKey(nArea.getStandardArea()))
						ancestors.put(nArea.getStandardArea(), nCurrent.getStandardArea());
				}else{
					double oldCost = nArea.getEstimateCost();
					//logger.info("OPEN.contains, or CLOSED contains Area(" + nArea.getID().getValue()
					//	+ ") oldCost = " + oldCost + ", newCost = " + newCost + " (in " + whilecount + " loop.)");
					//logger.debug("OPEN   = [" + OPEN + " ]");
					//logger.debug("CLOSED = [" + CLOSED + " ]");
					//logger.info("Re: newCost = " + newCost + " (from:" + nArea.getID().getValue() + ", to:" + goalArea.getID().getValue());
					//logger.info("oldCost = " + oldCost + " (from:" + nArea.getID().getValue() + ", to:" + goalArea.getID().getValue());
					if(newCost < oldCost){
						//logger.info("Update Cost: Area = " + nArea + ", oldCost = " + oldCost + ", newCost = " + newCost);
						//logger.info("**Current=[" + nCurrent.getID().getValue() + "]->To[" + area.getID().getValue() + "],newCost=" + newCost + ",oldCost=" + oldCost);
						nArea.setEstimateCost(newCost);
						ancestors.remove(nArea.getStandardArea());
						ancestors.put(nArea.getStandardArea(), nCurrent.getStandardArea());
						//logger.info("Update Ancestor: " + nArea + " => " + nCurrent);
						if(CLOSED.contains(nArea)){
							CLOSED.remove(nArea);
							OPEN.add(nArea);
							//logger.info("CLOSED.remove(" + nArea + "); OPEN.add(" + nArea + ");");
						}
					}
				}
			}
		}while(!found && !OPEN.isEmpty());
		if(!found){
			//logger.info("There's No Path From = " + startArea + " To = " + goalArea);
			return null;
		}
		//closedをリストにして返す
		Area current = (Area)(goal);
        List<EntityID> path = new LinkedList<EntityID>();
        do {
        	//logger.info("current = " + current);
            path.add(0, current.getID());
            current = ancestors.get(current);
            if (current == null) {
                throw new RuntimeException("Found a node with no ancestor! Something is broken.");
            }
        } while (current.getID() != start.getID());
        //logger.info("Path(AStar) to 956 is: " + path);
        return path;
	}
	
	private NAITOArea removeMinimumCostArea(List<NAITOArea> open){
		double minCost = Double.MAX_VALUE;
		int index = -1;
		for(int i = 0;i < open.size();i++){
			NAITOArea area = open.get(i);
			//logger.info("Area(" + area.getID().getValue() + ") EstimatedCost = " + area.getEstimateCost() + ", minCost = " + minCost);
			if(area.getEstimateCost() < minCost){
				minCost = area.getEstimateCost();
				index = i;
			}
		}
		if(index == -1)
			return null;
		//logger.info("Remove Minimum Cost Area = " + open.get(index) + ", OPEN=[" + open + "]");
		//logger.info("");
		return open.remove(index);
	}
	private double estimateCost(Map<Area, Area> ancestors, Area start, Area position, Area goal){
		String context = "Map<Area, Area> ancestors, " + start + ", " + position + ", " + goal + "";
		//logger.debug("+++++ estimateCost(" + context + ") +++++");
		if(ancestors.isEmpty()){
			//logger.debug("ancestors is Empty. return euclidDistance(); From start = " + start + ", To goal = " + goal);
			
			int startX = start.getX();
			int startY = start.getY();
			int endX   = goal.getX();
			int endY   = goal.getY();
			
			//logger.debug("+++++ estimateCost(" + context + ") end. +++++");
			return euclidDistance(start.getX(), start.getY(), goal.getX(), goal.getY());
		}
		
		//logger.debug("Estimate G Value. ");
		//logger.debug("ancestors = " + ancestors);
		
		
		int estimateG = 0;
		Area current = position; //neighbour;
		Area previous = null;
		do{
			previous = current;
			if(!ancestors.containsKey(current)){
				//logger.info("Not contains Key=" + current);
			}
			current = ancestors.get(current);
			//logger.debug("Start=" + start + ",Position=" + position + ",Goal=" + goal + ": Previous = " + previous + ", Current = " + current + ", call extimateGValue();");
			estimateG += estimateGValue(previous, current);
			//logger.debug("Current estimateG = " + estimateG);
		}while(current.getID().getValue() != start.getID().getValue());
		
		//logger.debug("Estimate H Value. Euclid Distance from CurrentArea's Center to GoalArea's Center.");
		int startX = position.getX();
		int startY = position.getY();
		int endX   = goal.getX();
		int endY   = goal.getY();
		double estimateH = euclidDistance(startX, startY, endX, endY);
		//logger.debug("estimateH = " + estimateH);
		
		//logger.debug("Estimate Result: G Value = " + estimateG + ", H Value = " + estimateH + "; return;");
		//logger.debug("+++++ estimateCost(" + context + ") end. +++++");
		double result = estimateG + estimateH;
		return result;
	}
	private double estimateGValue(Area previous, Area current){
		String context = previous + ", " + current;
		//logger.trace("----- estimateGValue(" + context + ") -----");
		if(previous.getID().getValue() == current.getID().getValue()){
			//logger.trace("Previous(" + previous.getID().getValue() + ") == Current(" + current.getID().getValue() + "). return 0;");
			return 0;
		}
		if(!previous.getNeighbours().contains(current.getID())){
			//logger.trace("Previous(" + previous.getID().getValue() + ") Neighbour is Not contains Current Area(" + current.getID().getValue() + "). return -1;");
			//logger.trace("----- estimateGValue(" + context + ") -----");
			return -1;
		}
		
		double cost = 0;
		
		//logger.trace("Estimate Cost from CurrentArea(" + current.getID().getValue() + "Center, to Edge(previous(" + previous.getID().getValue() + ")'s Center.");
		Edge edgeTo = current.getEdgeTo(previous.getID());
		
		int  startX = current.getX();
		int  startY = current.getY();
		int  edgeX  = (edgeTo.getEndX() + edgeTo.getStartX()) / 2;
		int  edgeY  = (edgeTo.getEndY() + edgeTo.getStartY()) / 2;
		//logger.trace("Length of (" + startX + ", " + startY + ") => (" + edgeX + ", " + edgeY + ")");
		
		cost = euclidDistance(startX, startY, edgeX, edgeY);
		
		//cost = euclidDistance(current, edgeTo);
		//logger.trace("Estimate Cost from Edge(previous(" + previous.getID().getValue() + "))'s Center, to PreviousArea's Center.");
		
		int endX = previous.getX();
		int endY = previous.getY();
		
		//logger.trace("Length of (" + edgeX + ", " + edgeY + ") => (" + endX + ", " + endY + ")");
		cost += euclidDistance(edgeX, edgeY, endX, endY);
		
		//cost += euclidDistance(edgeTo, previous);
		return cost;
	}
	
	private double euclidDistance(int startX, int startY, int endX, int endY){
		int dX = endX - startX;
		int dY = endY - startY;
		
		//logger.info("dx = " + dX + ", dy = " + dY + "=> startX = " + startX + ", startY = " + startY + ", endX = " + endX + ", endY = " + endY + " => result = " + (int) Math.hypot(dX, dY) + " <= dx = " + dX + ", dy = " + dY);
		//return (int) Math.sqrt((dX * dX) + (dY * dY));
		return (int) Math.hypot(dX, dY);
	}
	
	private int euclidDistance(StandardEntity e1, StandardEntity e2){
		int result = model.getDistance(e1, e2);
		//logger.info("euclidDistance(" + e1 + ", " + e2 + ") => result = " + result);
		return result;
	}
	private List<Area> getNeighbours(Area area){
		List<Area> result = new ArrayList<Area>();
		for(EntityID nID : area.getNeighbours()){
			Area a = (Area)(model.getEntity(nID));
			result.add(a);
		}
		return result;
	}
    /**
	   経路の幅優先探索
       Do a breadth first search from one location to the closest (in terms of number of nodes) of a set of goals.
       @param start The location we start at.
       @param goals The set of possible goals.
       @return The path from start to one of the goals, or null if no path can be found.
    */
    public List<EntityID> breadthFirstSearch(StandardEntity start, Collection<? extends StandardEntity> goals) {
        List<StandardEntity> open = new LinkedList<StandardEntity>();
        Map<StandardEntity, StandardEntity> ancestors = new HashMap<StandardEntity, StandardEntity>();
        open.add(start);
        StandardEntity next = null;
        boolean found = false;
        ancestors.put(start, start);
        do {
            next = open.remove(0);
            if (isGoal(next, goals)) {
                found = true;
                break;
            }
            Collection<StandardEntity> neighbours = findNeighbours(next);
            if (neighbours.isEmpty()) {
                continue;
            }
            for (StandardEntity neighbour : neighbours) {
                if (isGoal(neighbour, goals)) {
                    ancestors.put(neighbour, next);
                    next = neighbour;
                    found = true;
                    break;
                }
                else {                                                                           
                    if (!ancestors.containsKey(neighbour) && !(neighbour instanceof Building)){ 
                       //&& neighbour instanceof Road && !((Road)neighbour).isBlockadesDefined()) {
                        open.add(neighbour);
                        ancestors.put(neighbour, next);
                    }
                }
            }
        } while (!found && !open.isEmpty());
        if (!found) {
            // No path
            return null;
        }
        // Walk back from goal to start
        StandardEntity current = next;
        List<EntityID> path = new LinkedList<EntityID>();
        //        Logger.debug("Building path");
        //        Logger.debug("Goal found: " + current);
        do {
            path.add(0, current.getID());
            current = ancestors.get(current);
            //            Logger.debug("Parent node: " + current);
            if (current == null) {
                throw new RuntimeException("Found a node with no ancestor! Something is broken.");
            }
        } while (current != start);
        //        Logger.debug("Final path: " + path);
        return path;
    }

    /**
       Get the neighbours of an entity.
       @param e The entity to look up.
       @return All neighbours of that entity.
    */
    public Collection<StandardEntity> findNeighbours(StandardEntity e) {
        Collection<StandardEntity> result = new ArrayList<StandardEntity>();
        if (e instanceof Area) {
            Area a = (Area)e;
            for (EntityID next : a.getNeighbours()) {
                result.add(model.getEntity(next));
            }
        }
        return result;
    }

    private boolean isGoal(StandardEntity e, Collection<? extends StandardEntity> test) {
        for (StandardEntity next : test) {
            if (next.getID().equals(e.getID())) {
                return true;
            }
        }
        return false;
    }
	public String hogeString(){
		return owner.toString();
	}
}
